#include <string.h>
#include "accelerometer.h"
#include "adxl345.h"
#include "physics.h"
#include "uart.h"
#include "math.h"

/* acc offset */
double acc_bias[3] = {0,0,0};

double filter_acc[6][3];
/* accelerometer device */
acc_sensor_t acc = {0,0,0};
/**
 * Initialize accelerometer
 *
 * @param  none
 * @return none
 *
 * @brief  Initialize Acc HW/FW
 *
 */
void accelerometer_init(void)
{
    adxl345_init();
    accelerometer_calibrate();
}
/**
 * Calibrate accelerometer
 *
 * @param  none
 * @return none
 *
 * @brief  get a few samples and calculate middle value
 *
 */
void accelerometer_calibrate(void)
{
    int i;
    int16_t readings[3];

    for (i = 0; i < ADXL_CALIBRATION_SAMPLES; i++)
    {
        adxl345_read_xyz(readings);
        acc_bias[0] += readings[0];
        acc_bias[1] += readings[1];
        acc_bias[2] += readings[2];
    }

    acc_bias[0] /= ADXL_CALIBRATION_SAMPLES;
    acc_bias[1] /= ADXL_CALIBRATION_SAMPLES;
    acc_bias[2] /= ADXL_CALIBRATION_SAMPLES;

    acc_bias[2] -= 250;
}

/**
 * Sample Accelerometer
 *
 * @param  none
 * @return none
 *
 * @brief  get a few samples and calculate middle value
 *
 */
void accelerometer_sample(void)
{
    uint8_t i;
    int16_t readings[3];
    double temp[3];

    adxl345_read_xyz(readings);

    /* place filter over last 4 values */
    for (i=0; i<3;i++)
    {
        filter_acc[5][i] = filter_acc[4][i];
        filter_acc[4][i] = filter_acc[3][i];
        filter_acc[3][i] = filter_acc[2][i];
        filter_acc[2][i] = filter_acc[1][i];
        filter_acc[1][i] = filter_acc[0][i];
        filter_acc[0][i] = readings[i]- acc_bias[i];
        temp[i] = (filter_acc[0][i] + filter_acc[1][i] + filter_acc[2][i] + filter_acc[3][i] + filter_acc[4][i] + filter_acc[5][i])/6;

    }

    //ui_printf("%d %d %d\n", (int32_t)  temp[0], (int32_t) temp[1], (int32_t)  temp[2]);

    acc.a_x = temp[0] * ACCELEROMETER_GAIN;
    acc.a_y = temp[1] * ACCELEROMETER_GAIN;
    acc.a_z = temp[2] * ACCELEROMETER_GAIN;


}

/**
 * Get Accelerometer Pitch
 *
 * @param  none
 * @return none
 *
 * @brief  calculate pitch angle in degrees
 *
 */
double acc_get_pitch(void)
{
   return toDegrees(atan2(acc.a_x, acc.a_z));
}

/**
 * Get Accelerometer Roll
 *
 * @param  none
 * @return none
 *
 * @brief  calculate roll angle in degrees
 *
 */
double acc_get_roll(void)
{
   return toDegrees(atan2(acc.a_y, acc.a_z));
}

/**
 * Get Accelerometer Nick
 *
 * @param  none
 * @return none
 *
 * @brief  calculate yaw angle in degrees
 *
 */
double acc_get_yaw(void)
{
   return toDegrees(atan2(acc.a_x, acc.a_y));
}
